%***********************************************************
% CS 229 Machine Learning
% Project, Ground truth data generation
%-----------------------------------------------------------
% Date : November 26, 2010
%***********************************************************

clear all;

addpath(genpath('./LIBSVM/'))
addpath(genpath('./KalmanAll/'))

%**************************************************************************
%Input parameters

% Camera dimensions
%outputwidth = 320;
%outputheight = 180;
%Wmax = 640;
%Hmax = 360;

% Scales conversions
scale_conversion_factor_tracking = 1.5;
lecturer_tracking_scale_factor = 640/2560;
saliency_tracking_scale_factor = 640/1920;
board_scale_factor = 640/1920;

num_points = 50000;

% Kalman smoothing parameters
ground_truth_smooth_factor = 150;
final_output_smooth_factor = 250;

% Parameters for panning label prediction
frame = 500; % Sliding window size
numsteps = 5;
step = round(frame/numsteps); % Step size.
derivative_range = 30:30:150;

% Parameters for trajectory regression
frame_offset = 15900;
sample_step = frame_offset/10;

% SVM Parameters
%log2c = 0.95;
%log2g = 1.80;

% Panning noise removal parameters
pan_label_trough_width = 200;
pan_label_spike_width = 50;

% Rectification look ahead parameter
rectify_future_max = 500;

load groundTruth.dat;
load lecturer.dat;
load saliency.dat;
load boards.dat;
load panning.mat;
%**************************************************************************

dataA = groundTruth;

% The required frame rate is 33 ms per frame. Our data contains observations
% that are taken at time intervals that are multiples of 100 ms. Thus, we
% need to interpolate these observations.

size(dataA)

%--------------------------------------------------------------------------
% Interpolate data
interpolatedTimeStamps = (dataA(1,1):100/3:dataA(size(dataA,1),1));
newDataA = zeros(size(interpolatedTimeStamps',1),3);
newDataA(:,1) = interpolatedTimeStamps';
newDataA(:,2) = (interp1(dataA(:,1)', dataA(:,2)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;
newDataA(:,3) = (interp1(dataA(:,1)', dataA(:,3)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;

%--------------------------------------------------------------------------
%Adjustment for camera border case
%newDataA(:,2) = min(max(floor(newDataA(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2;
%newDataA(:,3) = min(max(floor(newDataA(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2 ;
%--------------------------------------------------------------------------

%==========================================================================================
%Prepare features


y = newDataA(:,2);
y = smooth(y,ground_truth_smooth_factor);
l = panning_indicator;
% y,l are the ground truth

%calculate board center in x-direction
board_center_x = (boards(:,1) + boards(:,2))/2;
board_center_x = board_center_x*board_scale_factor;

% Prepare features first

% Scale vectors
%lecuturer feature adjustment
lecturer = lecturer * lecturer_tracking_scale_factor;
%lecturer(:,2) = min(max(floor(lecturer(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2 ;
%lecturer(:,3) = min(max(floor(lecturer(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2  ;

%saliency feature adjustment
saliency = saliency *  saliency_tracking_scale_factor;

% Prepare all features
X_pan = PreparePanFeatures(1, num_points, frame, step, derivative_range, lecturer, saliency, boards, board_scale_factor);
X_pan = X_pan';
X_reg = PrepareFeatures(1, num_points, frame_offset, sample_step, lecturer, saliency, board_center_x);
X_reg = X_reg';

% MSE values:
MSE_before_panning_filter = 0;
MSE_after_panning_filter = 0;
MSE_final = 0;
sal_MSE = 0;
tracking_MSE = 0;

% Cross validation ranges
train_range = [1:40000; 1:30000 40001:50000; 1:20000 30001:50000; 1:10000 20001:50000; 10001:50000];
test_range = [40001:50000; 30001:40000; 20001:30000; 10001:20000; 1:10000];

% Code testing ranges
%train_range = [10001:50000];
%test_range = [1:10000];

bestc = 2^-1.1;
bestg = 2^0.9;
bestf = -1;
bestprecision = 0;
bestrecall = 0;

counter = 1;
for log2c = 13:1:15%-5:1:15
    for log2g = -15:1:3
        
        fmeasure = 0;
        totalprecision = 0;
        totalrecall = 0;
        counter
        counter = counter + 1;
        for seq_range=1:size(train_range,1)
            
            training_seq = train_range(seq_range,:);
            testing_seq = test_range(seq_range,:);
            
            % Step 1: Train the SVM to assign a panning label
            
            tic
            cmd = ['-t 2 -c ', num2str(2^log2c), ' -g ', num2str(2^log2g)];
            model =  svmtrain(l(training_seq), X_pan(training_seq,:) ,cmd);
            
            [predicted_panning_label, accuracy, decision_values] = svmpredict(l(testing_seq), X_pan(testing_seq,:), model);
            
            % The predicted panning labels are quite noisy in places where we expect a
            % '1'. Level these oscillations off to '1'. We do this in two steps:
            % 1. Go over the data once and change all groups of -1's which are thinner
            % than pan_label_trough_width frames in width to 1.
            % 2. Now, go over the data again, and remove all groups of 1's that are
            % less than pan_label_spike_width frames in width.
            
            last_one = 0;
            for i = 1:size(predicted_panning_label, 1)
                if (predicted_panning_label(i,1) == 1)
                    if (last_one > 0 && (i - last_one) <= pan_label_trough_width)
                        predicted_panning_label(last_one:i,1) = 1;
                    end
                    last_one = i;
                end
            end
            
            last_minus_one = 0;
            for i = 1:size(predicted_panning_label, 1)
                if (predicted_panning_label(i,1) == -1)
                    if (last_minus_one > 0 && (i - last_minus_one) <= pan_label_spike_width)
                        predicted_panning_label(last_minus_one:i,1) = -1;
                    end
                    last_minus_one = i;
                end
            end
            
            [precision, recall, accuracy] = computePrecisionRecall(predicted_panning_label, l(testing_seq));
            
            toc
            totalprecision = totalprecision + precision;
            totalrecall = totalrecall + recall;
            fmeasure = fmeasure + 2*(precision*recall)/(precision+recall);
        end
        
        fmeasure = fmeasure/5;
        totalprecision = totalprecision/5;
        totalrecall = totalrecall/5;
        
        if (fmeasure >= bestf),
            bestf = fmeasure; bestc = 2^log2c; bestg = 2^log2g;
            bestprecision = totalprecision;
            bestrecall = totalrecall;
            
            temp_result = [log2c, log2g ,fmeasure, bestc, bestg,  totalprecision, totalrecall];
            
            %in case crash/hang
            save('temp_result.mat','temp_result');
            
        end

    end
end

bestc
bestg
bestf
bestprecision
bestrecall

final_result = [bestc, bestg, bestf, bestprecision, bestrecall];
save('final_result.mat','final_result');
 %  t=testing_seq;
    
%     figure(10*seq_range + 1)
%     plot(t,50*l(t),t,50*predicted_panning_label, t,y(t)-mean(y(t)), t,saliency(t,2)-mean(saliency(t,2)), t, lecturer(t,2) - mean(lecturer(t,2)));
%     legend('Ground truth','Prediction','Actual Trajectory', 'saliency', 'lecturer')
%     title(['Precision:' num2str(precision) ' recall: ' num2str(recall) ' accuracy: ' num2str(accuracy) ]);
    